#include "./PORT/port.h"
#include "../System/config.h"
#include "./BSP/ENCODER/encoder.h"

const uint8_t encoder_tim_id[] = {CFG_ENCODER_TIM_ID};
const uint8_t encoder_pin[][2] = {CFG_ENCODER_PIN};

// 左右编码器电机
Encoder left_encoder =
    {
        .reverse = 0,
        .total_count = 0,
};
Encoder right_encoder =
    {
        .reverse = 1,
        .total_count = 0,
};

/**
 * @brief       编码器电机初始化函数
 * @note        该函数会初始化编码器电机的引脚和计数器
 * @retval      无
 */
void encoder_init(void)
{
    for (int i = 0; i < CFG_ENCODER_NUM; i++)
    {
        tim_cnt_pin_init(encoder_tim_id[i], 1, encoder_pin[i][0], 1);
        tim_cnt_pin_init(encoder_tim_id[i], 2, encoder_pin[i][1], 1);
        tim_cnt_set_value(encoder_tim_id[i], 0);
    }
}

/**
 * @brief       获取编码器计数值
 * @param       encoder_id: 编码器编号, 从0开始
 * @retval      编码器计数值
 */
short encoder_get_value(uint8_t encoder_id)
{
    Encoder *encoder = encoder_id == CFG_MOTOR_L_ID ? &left_encoder : &right_encoder;
    short value = tim_cnt_get_value(encoder_tim_id[encoder_id]);
    // 2. 处理编码器反向
    value = encoder->reverse == 0 ? value : -value;
    encoder->total_count += value;
    tim_cnt_set_value(encoder_tim_id[encoder_id], 0); // 重置计数器
    return value;
}
/**
 * @brief       获取编码器总计数值  用于计算走过的距离cm
 *
 * @param encoder_id    : 编码器编号, 从0开始
 * @return int32_t
 */
int32_t encoder_get_total_count(uint8_t encoder_id)
{
    Encoder *encoder = encoder_id == CFG_MOTOR_L_ID ? &left_encoder : &right_encoder;
    return encoder->total_count;
}
